#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QDebug>
#include <ahrs_driver.h>
#include <Eigen/Eigen>
#include <rclcpp/rclcpp.hpp>

#include <memory>
// #include <inttypes.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <string>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/time.hpp>
#include <QWidget>

using std::placeholders::_1;
int position_x;
int position_y;
int position_z;
double r_imu, p_imu, y_imu;
using namespace std::chrono_literals;





MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    // 开启UI界面
    ui->setupUi(this);
    // 设置imu的输出框无光标且只读
    ui->lineEdit->setStyleSheet("QLineEdit {}");
    ui->lineEdit->setReadOnly(true);
    ui->lineEdit_p->setStyleSheet("QLineEdit {}");
    ui->lineEdit_p->setReadOnly(true);
    ui->lineEdit_y->setStyleSheet("QLineEdit {}");
    ui->lineEdit_y->setReadOnly(true);


    // QString param_value = ui->param_updata->text();

    // 提示
    // qDebug()<<"读数线程开启!!!"<<Qt::endl;
    // 开启读数线程
    imu_data_Thread = new imu_data_thread(this);
    imu_data_Thread->start();
    // IMU广播线程（按键开启）
    imu_br_Thread = new imu_br_thread(this);
    // 开启参数读取线程
    parameters = new Parameters(this);
    parameters->start();  
    // 连接
    connect(imu_data_Thread,&imu_data_thread::Data,this,&MainWindow::display_imu);
    connect(ui->pushButton,SIGNAL(clicked(bool)),this,SLOT(On_allSelectBtnSlot()));
    connect(parameters,&Parameters::param_data,this,&MainWindow::display_param);


    // 开启参数修改线程
    parameters_updata = new Parameters_updata(this);
    parameters_updata->start();
    // parameters_updata->setData(ui->param_updata->text());
    // connect(parameters_updata,&Parameters_updata::finished,parameters_updata,&QObject::deleteLater);


}

MainWindow::~MainWindow()
{
    delete ui;
    rclcpp::shutdown();
}


//------------------------------------------------------------------------------------//

void MainWindow::On_allSelectBtnSlot()
{
    if(ui->pushButton->isChecked())
    {

        qDebug()<<"----------IMU使能----------"<<Qt::endl;
        // 开启线程
        imu_br_Thread->start();
    }
    else
    {
        qDebug()<<"----------IMU关闭----------"<<Qt::endl;
        // 关闭线程
        imu_br_Thread->terminate();
        // 清空lineedit中显示的数据
        ui->lineEdit->clear();
        ui->lineEdit_p->clear();
        ui->lineEdit_y->clear();
    }
}


void MainWindow::on_paramButton_clicked()
{
    qDebug()<<"开启-----------------------------------------------"<<Qt::endl;
    parameters_updata->setData(ui->param_updata->text());
    connect(parameters_updata,&Parameters_updata::finished,parameters_updata,&QObject::deleteLater);
    // parameters_updata->start();
    // if(parameters_updata->isRunning())
    // {
    //     qDebug()<<"线程存在"<<Qt::endl;
    // }
    // qDebug()<<"关闭-----------------------------------------------"<<Qt::endl;

}
// IMU广播线程任务
void imu_br_thread::run()
{
    qDebug() << "IMU Broadcast Thread Running"<<Qt::endl;
    FDILink::ahrsBringup bp;
}

// 读数线程任务
void imu_data_thread::run()
{
    qDebug()<<"读数线程开启"<<Qt::endl;


    // int argc=0;
    // char **argv=NULL;
    // rclcpp::init(argc,argv);
    node = rclcpp::Node::make_shared("imu_data_to_tf");
    sub_ = node->create_subscription<sensor_msgs::msg::Imu>("imu", 10, std::bind(&imu_data_thread::ImuCallback,this, _1));
    RCLCPP_INFO(node->get_logger(), "读数初始化完成");
    rclcpp::spin(node);
    rclcpp::shutdown();
    qDebug()<<"节点关闭"<<Qt::endl;

}


// 读数线程任务回调函数
void imu_data_thread::ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_data){

    // static tf2_ros::TransformBroadcaster br;//广播器

    // tf2::Transform transform;
    // transform.setOrigin(tf2::Vector3(position_x, position_y, position_z));//设置平移部分

    // 从IMU消息包中获取四元数数据
    // qDebug()<<"回调函数启动"<<Qt::endl;
    tf2::Quaternion q;
    q.setX(imu_data->orientation.x);
    q.setY(imu_data->orientation.y);
    q.setZ(imu_data->orientation.z);
    q.setW(imu_data->orientation.w);
    q.normalized(); // 归一化

    // transform.setRotation(q);//设置旋转部分
    // 广播出去
    // br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "imu"));
    geometry_msgs::msg::TransformStamped tfs;
    tfs.header.stamp = rclcpp::Clock().now();
    tfs.header.frame_id = "world";
    tfs.child_frame_id = "imu";
    tfs.transform.translation.x = position_x;
    tfs.transform.translation.y = position_y;
    tfs.transform.translation.z = position_z;
    tfs.transform.rotation.x = q.getX();
    tfs.transform.rotation.y = q.getY();
    tfs.transform.rotation.z = q.getZ();
    tfs.transform.rotation.w = q.getW();

    tf2::Matrix3x3(q).getRPY(r_imu, p_imu, y_imu);
    r_imu = r_imu*180/M_PI;
    p_imu = p_imu*180/M_PI;
    y_imu = y_imu*180/M_PI;


    // RCLCPP_INFO(node->get_logger(), "滚转：%.2f-----俯仰：%.2f-----偏航：%.2f-----", r_imu,p_imu,y_imu);
    // 通过信号槽将信号传出
    emit Data(r_imu,p_imu,y_imu);


}
// 将信号槽中的数据显示至Ui中
void MainWindow::display_imu(double dis_r,double dis_p,double dis_y){

    ui->lineEdit->setText(QString::number(dis_r));
    ui->lineEdit_p->setText(QString::number(dis_p));
    ui->lineEdit_y->setText(QString::number(dis_y));
}


// 显示参数线程任务
void Parameters::run()
{
    qDebug()<<"参数读取线程开启"<<Qt::endl;
    Mynode_client = rclcpp::Node::make_shared("Parameters_client_get");
    while(1)
    {
        sleep(1);
        param_client_ = std::make_shared<rclcpp::SyncParametersClient>(Mynode_client, "mynode_node_cpp_server");
        RCLCPP_INFO(Mynode_client->get_logger(), "参数客户端创建");
        int width = param_client_->get_parameter<int>("width");
        RCLCPP_INFO(Mynode_client->get_logger(), "width = %d", width);
        emit param_data(width);
    }

}
// 将参数显示至Ui中
void MainWindow::display_param(int dis_param)
{
    ui->param_display->setText(QString::number(dis_param));
}


// 修改参数线程任务
void Parameters_updata::run()
{
    while (1)
    {
        sleep(1);
        qDebug()<<"参数修改线程开启"<<Qt::endl;
        updata_client = rclcpp::Node::make_shared("Parameters_client_updata");
        param_client_updata = std::make_shared<rclcpp::SyncParametersClient>(updata_client, "mynode_node_cpp_server");
        // int width = param_client_updata->get_parameter<int>("width");
        // RCLCPP_INFO(updata_client->get_logger(), "width此处 = %d", width);
        int num_data = data_.toInt();
        param_client_updata->set_parameters({rclcpp::Parameter("width",num_data)});
        qDebug() << "Thread data:" << data_;
    }
    // qDebug()<<"参数修改线程开启"<<Qt::endl;
    // updata_client = rclcpp::Node::make_shared("Parameters_client_updata");
    // param_client_updata = std::make_shared<rclcpp::SyncParametersClient>(updata_client, "mynode_node_cpp_server");
    // // int width = param_client_updata->get_parameter<int>("width");
    // // RCLCPP_INFO(updata_client->get_logger(), "width此处 = %d", width);
    // int num_data = data_.toInt();
    // param_client_updata->set_parameters({rclcpp::Parameter("width",num_data)});
    // qDebug() << "Thread data:" << data_;
}














